# coding = utf-8

"""
用来将全局waypoint地图处理为pnc需要的地图
"""
from collections import deque
import math
import numpy as np

import carla

from agents.navigation.local_planner_behavior import RoadOption
from agents.tools.misc import compute_distance

class PNCPoint:
    def __init__(self, slen=0.0, left_width=2.0, right_width=2.0, cuv=0.0, kappa=0.0, dappa=0.0):
        self.slength = slen
        self.left_width = left_width # 左侧宽度
        self.right_width = right_width # 右侧宽度
        self.cuvature = cuv # 曲率
        self.kappa = kappa # 曲率变化速度
        self.dappa = dappa # 曲率变化加速度
        self.option = RoadOption.VOID
        self.speed = 10 # m/s 车道限速 

# def get_dist(wp0:carla.Waypoint, wp1:carla.Waypoint) -> float:
#     # wp0
#     x0 = wp0.transform.location.x
#     y0 = wp0.transform.location.y
#     # wp1
#     x1 = wp1.transform.location.x
#     y1 = wp1.transform.location.y
#     # value
#     return math.sqrt((x0-x1)**2 + (y0-y1)**2)

def get_curvature(wp0:carla.Waypoint, wp1:carla.Waypoint) -> float:
    """
    计算时，需要距离和转弯半径比很小，这样直接把两点的距离当成弧长
    """
    # 路点的朝向
    yaw0 = math.radians(wp0.transform.rotation.yaw)
    yaw1 = math.radians(wp1.transform.rotation.yaw)
    dyaw = yaw1 - yaw0
    while dyaw > math.pi:
        dyaw -= 2 * math.pi
    while dyaw < -math.pi:
        dyaw += 2* math.pi
    # 两点之间的距离
    dist = compute_distance(wp0.transform.location, wp1.transform.location)
    # 曲率
    return dyaw / dist

def get_pnc_map(global_trace:list, min_radius:float, sample_resolution:float):
    """
    将全局waypoint构成的地图转换为pnc用的sl地图
    :param global_trace : list, elem is [waypoint, roadoption]
    :param min_radius: 最小转弯半径
    @return
    :pnc_trace : list, elem is [waypoint, pncoption]
    """
    # 根据最小转弯半径和分辨率，可以计算出最大方向朝向角变化
    max_dyaw = sample_resolution / min_radius
    # 朝向角变化满足线性变化约束：即前后两个路点的方向角变化超过最大朝向角度时，
    # 假定此时满足最大朝向角变化，然后反推之前的路点需要满足的朝向角变化值
    # dyaw = k*i，其中，k表示最大朝向角变化速度，
    # 当第i点和第i-1点之间的角度变化为dyaw>max_dyaw时
    #
    # 根据最小转弯半径，取若干点做拟合 : 先按照10米计算，没太想明白最小转弯半径怎么用
    buff_size = math.ceil(10/sample_resolution)
    # 
    simulation_buffer = [0]*buff_size
    # 
    for i in range(buff_size):
        simulation_buffer[i] = global_trace
    pnc_trace = []
    wp0, wp0_option = global_trace[0]
    # 加入第2点时，计算
    for wp1, wp1_option in global_trace[1:]:
        slen += sample_resolution
        lw = 2.0
        rw = 2.0
        cuv = 0.0
        kappa = kappa
        dappa = dappa
        point = PNCPoint(slen=slen, left_width=lw, right_width=rw, cuv=cuv, kappa=kappa, dappa=dappa)
        point.option = wp1_option
    return pnc_trace

def get_waypoint_in_curvature(wp0:carla.Waypoint, waypoints: list, min_radius:float) -> carla.Waypoint:
    """
    根据转弯半径在路点列表中选择合适的路点
    """
    wp = waypoints[0]
    max_curvature = 1 / min_radius
    min_curv = 10000
    for wp1 in waypoints:
        # 与起始点计算曲率
        curv = get_curvature(wp0, wp1)
        # 如果找到转弯半径更大的点，即曲率更小的点，直接返回
        if curv < max_curvature:
            return wp1
        # 如果找不到，则找曲率尽可能小的点
        if curv < min_curv:
            min_curv = curv
            wp = wp1
    return wp
    